﻿using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Text;
using Microsoft.Kinect;
using SVU.Model;
using JointMapping = Microsoft.Samples.Kinect.WpfViewers.JointMapping;

namespace SVU.Managers
{
    public class AuthenticationManager
    {
        private const double _epsilon = .16;
        private const int UntrackedJointInt = 100;
        private static List<PersonBioID> _knownBioIDs;
        private static Queue<double> _lastMeasurements = new Queue<double>();
        private static PersonBioID _seenPerson = new PersonBioID();
        private static Stopwatch _sw = new Stopwatch();

        static AuthenticationManager()
        {
            _knownBioIDs = new List<PersonBioID>();

            var joseph = new PersonBioID();
            joseph.PersonName = "Joseph";
            joseph.Segments[JointType.WristLeft].SegmentLength = .26;
            joseph.Segments[JointType.WristRight].SegmentLength = .24;
            joseph.Segments[JointType.ElbowLeft].SegmentLength = .29;
            joseph.Segments[JointType.ElbowRight].SegmentLength = .27;
            joseph.Segments[JointType.AnkleLeft].SegmentLength = .39;
            joseph.Segments[JointType.AnkleRight].SegmentLength = .40;
            joseph.Segments[JointType.KneeLeft].SegmentLength = .52;
            joseph.Segments[JointType.KneeRight].SegmentLength = .52;
            joseph.Segments[JointType.ShoulderLeft].SegmentLength = .36;
            _knownBioIDs.Add(joseph);

            var michael = new PersonBioID();
            michael.PersonName = "Michael";
            michael.Segments[JointType.WristLeft].SegmentLength = .16;
            michael.Segments[JointType.WristRight].SegmentLength = .16;
            michael.Segments[JointType.ElbowLeft].SegmentLength = .19;
            michael.Segments[JointType.ElbowRight].SegmentLength = .19;
            michael.Segments[JointType.AnkleLeft].SegmentLength = .25;
            michael.Segments[JointType.AnkleRight].SegmentLength = .23;
            michael.Segments[JointType.KneeLeft].SegmentLength = .31;
            michael.Segments[JointType.KneeRight].SegmentLength = .30;
            michael.Segments[JointType.ShoulderLeft].SegmentLength = .22;
            _knownBioIDs.Add(michael);

            var landon = new PersonBioID();
            landon.PersonName = "Landon";
            landon.Segments[JointType.WristLeft].SegmentLength = .14;
            landon.Segments[JointType.WristRight].SegmentLength = .13;
            landon.Segments[JointType.ElbowLeft].SegmentLength = .16;
            landon.Segments[JointType.ElbowRight].SegmentLength = .14;
            landon.Segments[JointType.AnkleLeft].SegmentLength = .18;
            landon.Segments[JointType.AnkleRight].SegmentLength = .18;
            landon.Segments[JointType.KneeLeft].SegmentLength = .25;
            landon.Segments[JointType.KneeRight].SegmentLength = .25;
            landon.Segments[JointType.ShoulderLeft].SegmentLength = .22;
            _knownBioIDs.Add(landon);

            var emily = new PersonBioID();
            emily.PersonName = "Emily";
            emily.Segments[JointType.WristLeft].SegmentLength = .21; // L forearm
            emily.Segments[JointType.WristRight].SegmentLength = .22; // R forearm
            emily.Segments[JointType.ElbowLeft].SegmentLength = .27; // L upper arm
            emily.Segments[JointType.ElbowRight].SegmentLength = .22; // R upper arm
            emily.Segments[JointType.AnkleLeft].SegmentLength = .35; // L shin
            emily.Segments[JointType.AnkleRight].SegmentLength = .37; // R shin
            emily.Segments[JointType.KneeLeft].SegmentLength = .49; // L thigh
            emily.Segments[JointType.KneeRight].SegmentLength = .48; // R thigh
            emily.Segments[JointType.ShoulderLeft].SegmentLength = .34; // shoulder
            _knownBioIDs.Add(emily);

            var sarah = new PersonBioID();
            sarah.Id = 2;
            sarah.PersonName = "Sarah";
            sarah.Segments[JointType.WristLeft].SegmentLength = .23;
            sarah.Segments[JointType.WristRight].SegmentLength = .23;
            sarah.Segments[JointType.ElbowLeft].SegmentLength = .27;
            sarah.Segments[JointType.ElbowRight].SegmentLength = .27;
            sarah.Segments[JointType.AnkleLeft].SegmentLength = .38;
            sarah.Segments[JointType.AnkleRight].SegmentLength = .38;
            sarah.Segments[JointType.KneeLeft].SegmentLength = .47;
            sarah.Segments[JointType.KneeRight].SegmentLength = .47;
            sarah.Segments[JointType.ShoulderLeft].SegmentLength = .32;
            _knownBioIDs.Add(sarah);

            var spencer = new PersonBioID();
            spencer.Id = 2;
            spencer.PersonName = "Spencer";
            spencer.Segments[JointType.WristLeft].SegmentLength = .25;
            spencer.Segments[JointType.WristRight].SegmentLength = .22;
            spencer.Segments[JointType.ElbowLeft].SegmentLength = .24;
            spencer.Segments[JointType.ElbowRight].SegmentLength = .25;
            spencer.Segments[JointType.AnkleLeft].SegmentLength = .41;
            spencer.Segments[JointType.AnkleRight].SegmentLength = .39;
            spencer.Segments[JointType.KneeLeft].SegmentLength = .45;
            spencer.Segments[JointType.KneeRight].SegmentLength = .44;
            spencer.Segments[JointType.ShoulderLeft].SegmentLength = .38;
            _knownBioIDs.Add(spencer);

        }

        private static void AuthorizationReceived(PersonBioID personBioId)
        {
            if (SvuConstants.State != ProcessState.AuthenticatingMode)
                return;

            SvuConstants.State = ProcessState.Authentication_SuccessMode;
            FeedbackManager.UpdateStatus();
            FeedbackManager.AddFeedback("Successfully Authenticated: " + personBioId.PersonName);
            SvuConstants.AuthenticationCountDownTimer.Enabled = false;
            SvuConstants.CountDownTimerSecondsElapsed = 0;
            SvuConstants.AlarmPlayer.Stop();
        }

        public static void LookupPerson(Dictionary<JointType, JointMapping> jointMapping)
        {
            _seenPerson.Segments[JointType.WristLeft].SegmentLength =
                Calc3DDistance(jointMapping[JointType.WristLeft].Joint,
                               jointMapping[JointType.ElbowLeft].Joint);
            _seenPerson.Segments[JointType.WristRight].SegmentLength =
                Calc3DDistance(jointMapping[JointType.WristRight].Joint,
                               jointMapping[JointType.ElbowRight].Joint);
            _seenPerson.Segments[JointType.ElbowLeft].SegmentLength =
                Calc3DDistance(jointMapping[JointType.ElbowLeft].Joint,
                               jointMapping[JointType.ShoulderLeft].Joint);
            _seenPerson.Segments[JointType.ElbowRight].SegmentLength =
                Calc3DDistance(jointMapping[JointType.ElbowRight].Joint,
                               jointMapping[JointType.ShoulderRight].Joint);
            _seenPerson.Segments[JointType.AnkleLeft].SegmentLength =
                Calc3DDistance(jointMapping[JointType.AnkleLeft].Joint,
                               jointMapping[JointType.KneeLeft].Joint);
            _seenPerson.Segments[JointType.AnkleRight].SegmentLength =
                Calc3DDistance(jointMapping[JointType.AnkleRight].Joint,
                               jointMapping[JointType.KneeRight].Joint);
            _seenPerson.Segments[JointType.KneeLeft].SegmentLength =
                Calc3DDistance(jointMapping[JointType.KneeLeft].Joint,
                               jointMapping[JointType.HipLeft].Joint);
            _seenPerson.Segments[JointType.KneeRight].SegmentLength =
                Calc3DDistance(jointMapping[JointType.KneeRight].Joint,
                               jointMapping[JointType.HipRight].Joint);
            _seenPerson.Segments[JointType.ShoulderLeft].SegmentLength =
                Calc3DDistance(jointMapping[JointType.ShoulderLeft].Joint,
                               jointMapping[JointType.ShoulderRight].Joint);

            PersonBioID personBioId = MatchPerson();

            if (personBioId != null)
            {
                AuthorizationReceived(personBioId);
                FeedbackManager.UpdatePerson(personBioId.PersonName);
            }
            else
            {
                FeedbackManager.UpdatePerson("?");
            }
        }

        private static void FormatMeasurement(StringBuilder stringBuilder, JointType jointType,
                                              PersonBioID closestPerson)
        {
            string label = PersonBioID.SegmentNameMapping[jointType];
            double length = _seenPerson.Segments[jointType].SegmentLength;
            double diff = closestPerson == null
                              ? 0
                              : _seenPerson.Segments[jointType].SegmentLength -
                                closestPerson.Segments[jointType].SegmentLength;
            double closest = closestPerson == null ? 0 : closestPerson.Segments[jointType].SegmentLength;

            string diffS = diff.ToString("0.00");
            if (diff == 0)
                diffS = "";
            else if (Math.Abs(diff) > .03)
                diffS += "**";

            string closestS = closest == 0 ? "" : closest.ToString("0.00");

            stringBuilder.Append(string.Format("{0}:{1:0.00} close:{2} diff:{3}\r\n", label, length, closestS,
                                               diffS));
        }

        private static double Calc3DDistance(Joint joint1, Joint joint2)
        {
            if (joint1.TrackingState != JointTrackingState.Tracked || joint2.TrackingState != JointTrackingState.Tracked)
                return UntrackedJointInt;

            return
                Math.Sqrt(Math.Pow(joint2.Position.X - joint1.Position.X, 2) +
                          Math.Pow(joint2.Position.Y - joint1.Position.Y, 2) +
                          Math.Pow(joint2.Position.Z - joint1.Position.Z, 2));
        }

        private static PersonBioID MatchPerson()
        {
            _sw.Reset();
            _sw.Start();

            double lowestTotalDiff = UntrackedJointInt;
            PersonBioID closestPersonBioId = null;
            foreach (PersonBioID knownBioId in _knownBioIDs)
            {
                double totalDiff = 0;
                foreach (JointType jointType in PersonBioID.SegmentNameMapping.Keys)
                {
                    double diff = _seenPerson.Segments[jointType].SegmentLength -
                                  knownBioId.Segments[jointType].SegmentLength;
                    totalDiff += Math.Abs(diff);
                }

                if (totalDiff < lowestTotalDiff)
                {
                    lowestTotalDiff = totalDiff;
                    closestPersonBioId = knownBioId;
                }
            }


            _sw.Stop();

            // Display Person
            var measurement = new StringBuilder();

            foreach (JointType jointType in PersonBioID.SegmentNameMapping.Keys)
            {
                FormatMeasurement(measurement, jointType, closestPersonBioId);
            }
            measurement.Append(string.Format("lowest TOTAL DIFF: {0:0.000}\r\n", lowestTotalDiff));
            if (closestPersonBioId != null)
                measurement.Append("closest person:" + closestPersonBioId.PersonName + "\r\n");
            else
                measurement.Append("closest person: (unknown)\r\n");

            FeedbackManager.UpdatePersonMeasurement(measurement.ToString());

            if (lowestTotalDiff < _epsilon)
            {
                return closestPersonBioId;
            }
            else
            {
                return null;
            }
        }
    }
}